#include "decode_yuv420sp_nv21.h"
#include "tools.h"
#include <stdio.h>
using namespace std;

enum OUT_TYPE {
    OUT_INT = 1,
    OUT_CHAR = 2,
};

#define LOG(msg) do{printf("%s\n", msg);} while (0)

static void __yuv420sp_nv21_to_0rgb(void *out_argb, enum OUT_TYPE out_type, char yuv[], int width, int height) {
    //modified from 
    //http://stackoverflow.com/questions/12469730/confusion-on-yuv-nv21-conversion-to-rgb

    DURATION_START;

    if (OUT_INT == out_type) {
        LOG("OUT_INT passed");
    } else if (OUT_CHAR == out_type) {
        LOG("OUT_CHAR passed");
    }

    const int frameSize = width * height;

    const int ii = 0;
    const int ij = 0;
    const int di = +1;
    const int dj = +1;

    int a = 0;
    for (int i = 0, ci = ii; i < height; ++i, ci += di) {
        for (int j = 0, cj = ij; j < width; ++j, cj += dj) {
            int y = (0xff & ((int) yuv[ci * width + cj]));
            int v = (0xff & ((int) yuv[frameSize + (ci >> 1) * width + (cj & ~1) + 0]));
            int u = (0xff & ((int) yuv[frameSize + (ci >> 1) * width + (cj & ~1) + 1]));
            y = y < 16 ? 16 : y;

            int r = (int) (1.164f * (y - 16) + 1.596f * (v - 128));
            int g = (int) (1.164f * (y - 16) - 0.813f * (v - 128) - 0.391f * (u - 128));
            int b = (int) (1.164f * (y - 16) + 2.018f * (u - 128));

            r = r < 0 ? 0 : (r > 255 ? 255 : r);
            g = g < 0 ? 0 : (g > 255 ? 255 : g);
            b = b < 0 ? 0 : (b > 255 ? 255 : b);

            //cout << hex << r << ", " << hex << g << "," << hex << b << endl;

            //set alpha to 0
            if (OUT_INT == out_type) {
                ((int *)out_argb)[a++] = 0x00000000 | r << 16 | g << 8 | b;
            } else if (OUT_CHAR == out_type) {

                ((char *)out_argb)[a ++] = b;
                ((char *)out_argb)[a ++] = g;
                ((char *)out_argb)[a ++] = r;
                ((char *)out_argb)[a ++] = 0;

            }
        }
    }
    DURATION_STOP("converting yuv to rgb");
}

void decodeYuv420sp_0rgb(int argb[], char yuv[], int width, int height) {
    __yuv420sp_nv21_to_0rgb((void *)argb, OUT_INT, yuv, width, height);
}

void decodeYuv420sp_bgr0(char argb[], char yuv[], int width, int height) {
    __yuv420sp_nv21_to_0rgb((void *)argb, OUT_CHAR, yuv, width, height);
}
